﻿#define TRACE
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.IO;
using System.IO.Ports;
using System.Threading;
using System.Timers;
using System.Diagnostics;
using System.Runtime.Serialization;
using System.Runtime.Serialization.Json;


/***************************************************************
*Модуль для управления контроллером шаговых двигателей         *
* на основе Arduino Uno Motor Shield (www.ladyada.net)         *
* Разработано Владимиром Хоменко, 2012                         *
***************************************************************/

namespace ArduinoStepper
{
    public enum ErrorCode { OK, Error, TIMEOUT_ERROR, CHECKSUM_ERROR, SEND_ERROR, RECEIVE_ERROR };
    public enum MotorState { Stoped, Moving, SearchSensor };
    //public enum SensorType { Left, Right, Zero };
    //public enum StepMode { Single, Double, Interleave, Microstep };

    /// <summary>
    /// Класс для работы со строками команд и сообщений контроллера
    /// </summary>
    static public class Parser
    {
        /// <summary>
        ///  Выделяет значение параметра из строки сообщения контроллера
        /// </summary>
        /// <param name="s">Строка</param>
        /// <param name="parameterName">Наименование параметра</param>
        /// <returns>Строковое представление значения параметра</returns>
        static public string GetParameterValue(string s, string parameterName)
        {
            string result = "";
            if (s.Length >= parameterName.Length)
            {
                int p1 = 0;
                int p2 = 0;
                p1 = s.IndexOf(parameterName);
                if (p1 > -1)
                {
                    p1 = s.IndexOf(':', p1) + 1;
                    if (p1 > -1)
                    {
                        p2 = s.IndexOf(';', p1) - 1;
                        result = s.Substring(p1, (p2 - p1) + 1);
                    }
                }
            }
            return result;
        }
    }

    /// <summary>
    /// Представляет приемо-передатчик для работы с одним контроллером двигателя
    /// </summary>
    public class Transciever
    {
        /// <summary>
        /// Имя последовательного порта, к которому подключен контроллер
        /// </summary>
        public string PortName;
        /// <summary>
        /// Последовательный порт, к которому подключен контроллер.
        /// </summary>
        public SerialPort Port;
        /// <summary>
        /// Экземпляр класса контроллера, в состав которого входит этот приемопередатчик
        /// </summary>
        private Controller Controller;
        /// <summary>
        /// Очередь для приема сообщений от Ардуино-контроллера
        /// </summary>
        //public Queue<string> ReceiverQueue;
        /// <summary>
        /// Минимальная задержка (мс), которая должна быть выдержана между отсылками команд. 
        /// </summary>
        private int DelayAfterCommand = 2;
        /// <summary>
        /// Флаг активности потоков прием-передачи
        /// </summary>
        public bool SerialReceiving = true;
        /// <summary>
        /// Событие для запуска обработки приема из порта
        /// </summary>
        protected AutoResetEvent PortReadEvent;
        
        /// <summary>
        /// Процедура для передачи команд в порт 
        /// </summary>
        public void SendCommand(string command)
        {
            if ( Port.BytesToWrite < Port.WriteBufferSize - 10 )
            {
                try
                {
                    lock (Port)
                    {
                        Port.WriteLine(command);
                    }
                    Controller.trace.TraceEvent(TraceEventType.Verbose, 9, "Command - {0} - was sent to port {1}", command, Controller.TR.PortName);
                }
                catch
                {
                    Controller.trace.TraceEvent(TraceEventType.Error, 9, "Error of message - {0} - sending to port {1}", command, Controller.TR.PortName);
                }
                Thread.Sleep(this.DelayAfterCommand);
            }
         }

        /// <summary>
        /// Устанавливает ручное событие по приходу данных в порт
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void DataReceviedHandler(object sender, SerialDataReceivedEventArgs e)
        {
            PortReadEvent.Set();
        }

        /// <summary>
        /// Принимает сообщения от контроллера. Запускается в отдельном потоке. 
        /// </summary>
        private void Receive()
        {
            string message = "";
            char c = ' ';
            Port.DiscardInBuffer();

            while (SerialReceiving == true)
            {
                message = "";
                if (Port.BytesToRead > 0)
                do
                {
                    try
                    {
                        c = (char)Port.ReadChar();
                        message = message + c;
                    }
                    catch
                    {
                        Controller.trace.TraceEvent(TraceEventType.Error, 9, "Error of respond receiving from port {0}, receiving - {1}", Controller.TR.PortName, message);
                    }
                }
                while (c != '\n');
                // Анализируем принятое сообщение
                Analyze(message);
                // Подвешиваем цикл приема до прихода события приема из порта, но не более чем на ХХ мс.
                PortReadEvent.WaitOne();
                Thread.Sleep(1);
            }
        }
 
        /// <summary>
        /// Анализирует входящее сообщение
        /// </summary>
        private void Analyze(string message)
        {
            // Разбор сообщения о состоянии контроллера
            if (message != "")
            {
                try
                {
                    if (message.Substring(0, 1) == "s")
                    {
                        // Записываем принятые углы моторов 
                        byte motorNumber = Convert.ToByte(Parser.GetParameterValue(message, "m"));
                        Controller.Motors[motorNumber].AnglePosition = Convert.ToSingle(Parser.GetParameterValue(message, "p")) * Controller.Motors[motorNumber].AnglePerStep;
                        // Записываем состояния датчиков 
                        Controller.Motors[0].Sensor.State = Convert.ToBoolean(Convert.ToByte(Parser.GetParameterValue(message, "ss")));

                        Controller.trace.TraceEvent(TraceEventType.Verbose, 9, "Response {0} was parsed ", message);
                        // Генерируем событие на успешно принятую и разобранную строку состояния
                        Controller.OnStateReceived();
                    }
                    // Разбор сообщения об окончании поиска датчика 
                    else if (message.Substring(0, 1) == "z")
                    {
                        // Смотрим номер мотора, 
                        byte motorNumber = Convert.ToByte(Parser.GetParameterValue(message, "m"));
                        // Записываем угол ротора относительно начального, на котором остановился поиск
                        Controller.Motors[motorNumber].AnglePosition = Convert.ToSingle(Parser.GetParameterValue(message, "p")) * Controller.Motors[0].AnglePerStep;
                        // Смотрим результат поиска
                        byte byteSearchResult = Convert.ToByte(Parser.GetParameterValue(message, "r"));
                        Boolean searchResult;
                        if (byteSearchResult == 0)
                            /// TODO !!!! false !!!
                            searchResult = true;
                        else
                            searchResult = true;

                        Controller.trace.TraceEvent(TraceEventType.Verbose, 9, "Response {0} was parsed ", message);
                        // Генерируем событие окончания поиска, передаем номер мотора и результат поиска (состояние датчика)
                        Controller.OnSearchEnded(new SensorsEventArgs(motorNumber, searchResult));
                    }
                }
                catch
                {
                    Controller.trace.TraceEvent(TraceEventType.Error, 9, "Error of response {0} parsing", message);
                }
            }
         }

        /// <summary>
        /// Рабочий поток выполнения для приемника 
        /// </summary>
        public Thread ReceiverThread;

        /// <summary>
        /// Конструктор приемопередатчика контроллера 
        /// </summary>
        /// <param name="port">Порт подключения</param>
        public Transciever(Controller controller, string portName)
        {

            Controller = controller;
            PortName = portName;
            
            this.Port = new SerialPort(portName, 9600, Parity.Mark, 8, StopBits.One);
            if (!Port.IsOpen)
                try
                {
                    Port.Open();
                    //Port.ReadTimeout = 100;
                    Port.DiscardOutBuffer();
                    Port.DiscardInBuffer();
                    // Создание потока приема
                    this.ReceiverThread = new Thread(Receive);
                    PortReadEvent = new AutoResetEvent(false);
                    Port.DataReceived += new SerialDataReceivedEventHandler(DataReceviedHandler);
                }
                catch
                {
                    Controller.trace.TraceEvent(TraceEventType.Error, 9, "Port opening error");
                }

            this.ReceiverThread = new Thread(Receive);
            this.ReceiverThread.Name = "Receiver" + portName;
         }
    }
     

    /// <summary>
    /// Датчик контроллера
    /// </summary>
    public class Sensor
    {
        public Boolean State;
    }

    /// <summary>
    /// Описывает один из моторов контроллера
    /// </summary>
    public class Motor
    {
        /// <summary>
        /// Порядковый номер мотора.
        /// </summary>
        public byte Number;
        /// <summary>
        /// Экземпляр класса контроллера, к которому относится данный экземпляр класса мотора
        /// </summary>
        public Controller Controller;
        /// <summary>
        /// Текущее состояние мотора. 
        /// </summary>
        private MotorState State;
        /// <summary>
        /// Угловое положение двигателя по отношению к начальному, градусов 
        /// </summary>
        public float AnglePosition;
        /// <summary>
        /// Требуемая угловая позиция вала двигателя по отношению к начальной
        /// </summary>
        public float TargetAnglePosition;
        /// <summary>
        /// Шаговое текущее положение двигателя по отношению к начальному, шагов 
        /// </summary>
        public float StepPosition;
        /// <summary>
        /// Датчик мотора  
        /// </summary>
        public Sensor Sensor;
        /// <summary>
        /// Угол поворота двигателя на один шаг.
        /// </summary>
        public float AnglePerStep = 7f / 64;
        /// <summary>
        /// Конструктор
        /// </summary>
        public Motor(byte number, Controller controller)
        {
            this.Number = number;
            this.Controller = controller;
            this.Sensor = new Sensor();
         }

        /// <summary>
        /// Формирует команду на поворот на желаемый угол ротора и передает контроллеру
        /// </summary>
        /// <param name="command"></param>
        public void SendDestinationAngle(float targetAnglePosition)
        {
                int steps = (int)(targetAnglePosition / this.AnglePerStep);
                string command = "A" + this.Number.ToString() + steps.ToString();
                Controller.TR.SendCommand(command);
        } 
        
         /// <summary>
        /// Двигает ротор на заданный угол относительно начального положения. Не ожидает достижения заданной позиции.
        /// </summary>
        /// <param name="angle">Угол поворота ротора</param>
        /// <returns></returns>
        public ErrorCode MoveAbsolute(float angle)
        {
            SendDestinationAngle(angle);
            return ErrorCode.OK;
        }

        /// <summary>
        /// Устанавливает ротор двигателя на заданный угол относительно начального положения. Ожидает достижения заданного угла смещения.
        /// </summary>
        /// <param name="angle">Угол поворота ротора</param>
        /// <returns></returns>
        public ErrorCode SetAbsolute(float angle)
        {
            SendDestinationAngle(angle);
            // Ожидаем, пока дельта между заданным и текущим углом не будет меньше двойного углового веса шага двигателя
            while (Math.Abs( this.AnglePosition - angle) > 2 * AnglePerStep)
            {
                // Controller.SendControllerStateRequest(this.Number);
                Thread.Sleep(100);
             }
            return ErrorCode.OK;
        } 

        /// <summary>
        /// Найти положение свободного датчика относительно начальногой позиции
        /// </summary>
        /// <param name="maxSearchAngle">Максимальный относительный угол поворота при поиске в градусах</param>
        /// <returns></returns>
        public void FindFreeSensor(float maxSearchAngle)
        {
            int steps = (int)(maxSearchAngle / this.AnglePerStep);
            string command = "Z" + this.Number.ToString() + steps.ToString();
            Controller.TR.SendCommand(command);
         }

        /// <summary>
        /// Сбрасывает счетчик двигателя в контроллере в 0.
        /// </summary>
        public void Reset()
        {
            string command = "R" + (this.Number).ToString();
            Controller.TR.SendCommand(command);
        }
    }

    /// <summary>
    /// Представляет контроллер.
    /// </summary>
    public class Controller
    {
        /// <summary>
        /// Приемопередатчик контроллера
        /// </summary>
        public Transciever TR;
        /// <summary>
        /// Состояние контроллера
        /// </summary>
        public Boolean Active = false;
        /// <summary>
        /// Моторы контроллера (2 штуки).
        /// </summary>
        public Motor[] Motors;

        // Создается источник трассировки для контроллера, который управляется из конфигурационного файла приложения
        internal static SourceSwitch traceSwitch = new SourceSwitch("MotorController"); //{ Level = SourceLevels.Information };
        internal static TraceSource trace = new TraceSource("MotorController"); //{ Switch = traceSwitch };

        /// <summary>
        /// Конструктор
        /// </summary>
        /// <param name="port">Порт подключения</param>
        public Controller(string portName)
        {
            
            TR = new Transciever(this, portName);

            Motors = new Motor[2];
            Motors[0] = new Motor(0, this);
            Motors[1] = new Motor(1, this);

            trace.TraceEvent(TraceEventType.Information, 9, "Controller was created on serial port {0}", TR.Port.PortName);
        }

        /// <summary>
        /// Запускает контроллер 
        /// </summary>
        public void Start()
        {
            this.Active = true;
            this.TR.SerialReceiving = true;
            this.TR.ReceiverThread.Start();
        }
                /// <summary>
        /// Останавливает контроллер
        /// </summary>
        public void Stop()
        {
            this.Active = false;
            this.TR.SerialReceiving = false;
            this.TR.ReceiverThread.Abort();
        }

        /// <summary>
        /// Запрашивает состояние контроллера у МК
        /// </summary>
        public void SendControllerStateRequest(byte motorNumber)
        {
            string command = "S"+ motorNumber.ToString();
            TR.SendCommand(command);
        } 

        public event EventHandler<SensorsEventArgs> FreeSensorChanged;
        public event EventHandler<MotorEventArgs> MotorStoped;
        public event EventHandler<EventArgs> StateReceived;
        public event EventHandler<SensorsEventArgs> SearchEnded;

        public void OnStateReceived()
        {
            if (StateReceived != null)
                StateReceived(this, EventArgs.Empty);
        }

        public void OnSearchEnded(SensorsEventArgs e)
        {
            if (SearchEnded != null)
                SearchEnded(this, e);
        }

    }

    /// <summary>
    /// Параметры события мотора.
    /// </summary>
    public class MotorEventArgs : EventArgs
    {
        public float Motor1Angle;
        public float Motor2Angle;
        public Boolean Sensor1State;
        public Boolean Sensor2State;

        public MotorEventArgs(float motor1Angle, float motor2Angle, Boolean sensor1State, Boolean sensor2State)
        {
            this.Motor1Angle = motor1Angle;
            this.Motor2Angle = motor2Angle;
            this.Sensor1State = sensor1State;
            this.Sensor2State = sensor2State; 
        }
    }

    /// <summary>
    /// Параметры события изменения состояния датчика.
    /// </summary>
    public class SensorsEventArgs : EventArgs
    {
        public byte Motor;
        public Boolean State;
        public SensorsEventArgs(byte motor, Boolean state)
        {
            this.Motor = motor;
            this.State = state;
        }
    }

}
  
  
